/***************************************************************************
 创建者: 华磊
 开始时间: 2018.4.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2018.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "domparser.h"
#include "ultrasonicsensor.h"
#include <qdebug.h>
#include <qstring.h>
#include <QMutexLocker>
#include "qdom.h"
#include <unistd.h>
#include <qiterator.h>
#include "MacroDefine/GeneralDefine.h"
#include "messagelog.h"
#include <QSerialPort>
#include <QSerialPortInfo>

//class UltrasonicSensorTest;
/**
 * @brief The SerialPortConfig_ultrasonic struct ,超声波模块串口参数
 */
struct SerialPortConfig_ultrasonic:QSerialPort
{
    int busType;
    QString serialDeviceName;
    int isUseUsbDeviceSerialCode;
    BaudRate baudRate;
    DataBits dataBits;
    Parity   parity;
    StopBits stopBits;
    int updatePeriod;

};



#define D_DEFAULT_RS485_ADDRESS     (0xe8)
#define D_MAX_CONSUM_TIME           (87)
UltrasonicSensor::UltrasonicSensor(QString configFilePath)
{
    barrierSlowDownDistance=0.3;
    barrierSlowDownRatio=0.2;
    barrierStopDistance =0.05;
    isInitialOk = false;
    isDetectEnable=true;
    debugFlag=0;


    sensorSafeStatus.resize(4);//支持４个方位
    validAddressList << 0xd0 << 0xd2 << 0xd4 << 0xd6 << 0xd8 \
                     << 0xda << 0xdc << 0xde << 0xe0 << 0xe2 \
                     << 0xe4 << 0xe6 << 0xe8 << 0xea << 0xec \
                     << 0xee << 0xf8 << 0xfa << 0xfc << 0xfe;

    serialConfig = new SerialPortConfig_ultrasonic;
    deviceNode = new UltrasonicDeviceNode;
    serialPort_device=new QSerialPort;

    int tmpKey=initialSensorByConfigFile(configFilePath);

    // 初始化设备
    if(1==tmpKey)
    {
        initalSerial();
        if( serialPort_device->isOpen() )
        {
            iterator_currentNode = deviceNodeMap.begin();
            while(iterator_currentNode != deviceNodeMap.end() )
            {
                qDebug() << "deviceNodeMap.size() :" << deviceNodeMap.size();
                qDebug() << "sensorDistanceValue:" << deviceNodeMap.value(iterator_currentNode.key()).deviceId;
                mutex.lock();
                sensorDistanceValue.insert(deviceNodeMap.value( iterator_currentNode.key() ).deviceId, 0);
                sensorEnable.insert(deviceNodeMap.value( iterator_currentNode.key() ).deviceId, true);
                mutex.unlock();
                iterator_currentNode++;
            }
            timer.start(serialConfig->updatePeriod);
            QObject::connect(&timer, SIGNAL(timeout()), this, SLOT(timeOut_slot()) );
            QObject::connect(serialPort_device ,SIGNAL(readyRead()) ,this ,SLOT( receiveDetectData() ));
        }
        else
        {
            isInitialOk = false;
            qDebug() << "串口未打开!";
            addMsg(ENUM_MSG_ERROR,"UltrasonicSensor","UltrasonicSensor",3851,-1);
        }
        iterator_currentNode = deviceNodeMap.begin();
    }
    else
    {
        qDebug() << "error,initialSensorByConfigFile ";
        addMsg(ENUM_MSG_ERROR,"UltrasonicSensor","UltrasonicSensor",3850,-1);
    }

}

int UltrasonicSensor::setDebugFlag(int debugFlagIn)
{
    debugFlag=debugFlagIn;
    return 1;
}

int UltrasonicSensor::setNewCollisionParameter(double barrierSlowDownDistanceIn, double barrierSlowDownRatioIn,
                                               double barrierStopDistanceIn)
{
    barrierSlowDownDistance=barrierSlowDownDistanceIn*1000;
    barrierSlowDownRatio=barrierSlowDownRatioIn;
    barrierStopDistance=barrierStopDistanceIn*1000;
    return 1;
}

int UltrasonicSensor::setUltrasonicSensorCheckEnable(bool isOn)
{
    isDetectEnable=isOn;
    return 1;
}

int UltrasonicSensor::getUltrasonicSensorSafeStatus(std::vector<double> &safeStatusOut, bool &isCheckEnableOut)
{
    mutex.lock();
    isCheckEnableOut=isDetectEnable;
    safeStatusOut=sensorSafeStatus;
    mutex.unlock();
    return 1;
}

bool UltrasonicSensor::isInitialOkStatus()
{
    return isInitialOk;
}

int UltrasonicSensor::setSensorWork(int locationIn, bool isEnable)
{
    mutex.lock();
    QMapIterator<int, bool> i(sensorEnable);
    while (i.hasNext())
    {
        i.next();
        if(locationIn==deviceNodeMap.value(i.key()).location)
        {
            sensorEnable[i.key()]=isEnable;
        }
    }

    mutex.unlock();

    return 1;
}



/**
 * @brief UltrasonicSensor::initialSensorByConfigFile   读取配置文件
 * @param configFilePath        配置文件路径
 * @return  成功返回１，失败返回负值
 */
int UltrasonicSensor::initialSensorByConfigFile(QString configFilePath)
{
    DomParser domparser;
    qDebug()<<"UltrasonicSensor::initialSensorByConfigFile ==== 读取配置文件:" << configFilePath;

    // 只读方式打开超声波传感器模块配置文件
    bool ok = domparser.openXml(configFilePath,QIODevice::ReadOnly);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasoniSensor::initialSensorByConfigFile ===== 配置文件打开失败:" << configFilePath;
        }

        return -1;
    }

    QDomNode typeNode = domparser.findSubNode(domparser.getRootDomElement(), "SerialSetting", ok );
    if( !ok )
    {
        qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读配置文件失败:" << configFilePath;
        return -2;
    }

    // 读取总线类型
    domparser.readXml( domparser.findSubNode(typeNode, "busType", ok), serialConfig->busType );
    if( !ok  )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ====读配置文件失败: busType";
        }
        return -3;
    }

    // 读取串口设备文件
    domparser.readXml(domparser.findSubNode(typeNode, "serialDeviceName", ok), serialConfig->serialDeviceName );
    if( !ok  )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasonicSensor::initialSensorByCoConfigFile 读取串口设备文件失败:"  << serialConfig->serialDeviceName;
        }
        return -4;
    }

    // 读取usb识别
    domparser.readXml(domparser.findSubNode(typeNode, "isUseUsbDeviceSerialCode", ok), serialConfig->isUseUsbDeviceSerialCode );
    if( !ok  )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasonicSensor::initialSensorByCoConfigFile 读取串口设备文件失败:"  << serialConfig->isUseUsbDeviceSerialCode;
        }
        return -4;
    }


    // 读取配置文件波特率
    int baud;
    domparser.readXml(domparser.findSubNode(typeNode, "baud" ,ok), baud);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读取串口配置文件波特率失败:"<< configFilePath;

        }
        return -6;
    }
    serialConfig->baudRate = (QSerialPort::BaudRate)baud;

    // 读取配置文件校验位
    QString parity;
    int parityNumber;
    domparser.readXml(domparser.findSubNode(typeNode, "parity", ok), parity);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasonicSensor::initialSensorCommandgFile ==== 读取串口配置文件校验位失败:" << configFilePath;
        }
        return -7;
    }

    if(parity == "NONE")
    {
        parityNumber = 0;
    }
    else if (parity == "EVEN")
    {
        parityNumber = 2;
    }
    else if (parity == "ODD")
    {
        parityNumber= 3;
    }
    else
    {
        qDebug()  << "UltrasonicSensor::initialSensorByConfigFile ==== 校验位设置错误:" << configFilePath;
        return -7;
    }

    serialConfig->parity = (QSerialPort::Parity)parityNumber;

    //　读取配置文件数据位
    int dataBits;
    domparser.readXml(domparser.findSubNode(typeNode, "data_bit", ok), dataBits);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读取串口配置文件数据位失败:" << configFilePath;
        }
        return -8;
    }
    serialConfig->dataBits = (QSerialPort::DataBits)dataBits;

    // 读取配置文件停止位
    int stopBits;
    domparser.readXml(domparser.findSubNode(typeNode, "stop_bit", ok), stopBits );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读取串口配置文件停止位失败:" << configFilePath;
        }
        return -9;
    }
    serialConfig->stopBits = (QSerialPort::StopBits)stopBits;

    domparser.readXml(domparser.findSubNode(typeNode, "updatePeriod", ok), serialConfig->updatePeriod);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读取串口配置文件数据更新频率失败:" << configFilePath;
        }
        return -10;
    }

    QDomNode  nodeInfoNode = domparser.findSubNode(domparser.getRootDomElement(), "nodeInfo", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"UltrasonicSensor::initialSensorByConfigFile ==== 读设备节点失败: "<< configFilePath;
        }
        return -1;
    }

    //read barrier define
    typeNode = domparser.findSubNode(domparser.getRootDomElement(), "barrierSlowDownDistance", ok );
    if( !ok )
    {
        qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读配置文件失败:" << configFilePath;
        return -2;
    }
    // 读barrierSlowDownDistance
    domparser.readXml( typeNode, barrierSlowDownDistance );
    if( !ok  )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ====读配置文件失败: barrierSlowDownDistance";
        }
        return -3;
    }
    typeNode = domparser.findSubNode(domparser.getRootDomElement(), "barrierSlowDownRatio", ok );
    if( !ok )
    {
        qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读配置文件失败:" << configFilePath;
        return -2;
    }
    // barrierSlowDownRatio
    domparser.readXml( typeNode, barrierSlowDownRatio );
    if( !ok  )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ====读配置文件失败: barrierSlowDownRatio";
        }
        return -3;
    }
    typeNode = domparser.findSubNode(domparser.getRootDomElement(), "barrierStopDistance", ok );
    if( !ok )
    {
        qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读配置文件失败:" << configFilePath;
        return -2;
    }
    // barrierStopDistance
    domparser.readXml( typeNode, barrierStopDistance );
    if( !ok  )
    {
        if(IF_DEBUG)
        {
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ====读配置文件失败: barrierStopDistance";
        }
        return -3;
    }


    // 读取设备
    UltrasonicDeviceNode deviceNode;
    QDomNodeList deviceNodeList = nodeInfoNode.childNodes();
    for(int i = 0; i < deviceNodeList.count(); ++i )
    {
            qDebug() << "deviceNodeList.count()" << deviceNodeList.count();
            // 读取设备Id
            domparser.readXml(domparser.findSubNode(deviceNodeList.at(i), "deviceId", ok), deviceNode.deviceId);
            if( !ok )
            {
                if(IF_DEBUG)
                {
                    qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读取deviceId失败:" \
                             << deviceNode.deviceId;
                }
                return -11;
            }
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ====deviceId:" << deviceNode.deviceId;

            // 读取电源环境
            int powterNoise;
            domparser.readXml(domparser.findSubNode(deviceNodeList.at(i), "powerNoiseClass" ,ok), powterNoise);
            if( !ok )
            {
                if(IF_DEBUG)
                {
                    qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读取设备powerNoiseClass失败:" \
                             << powterNoise;
                }
                return -12;
            }

            deviceNode.powerNoiseClass = (EM_POWER_NOISE_CLASS)powterNoise;
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ====powerNoiseClass:" << deviceNode.powerNoiseClass;

            // 读取探测范围
            domparser.readXml(domparser.findSubNode(deviceNodeList.at(i), "detectRange" ,ok), deviceNode.datectRange );
            if( !ok )
            {
                if(IF_DEBUG)
                {
                    qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读取设备探测范围失败:" \
                             << deviceNode.datectRange;
                }
                return -13;
            }
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ====detectRange:" << deviceNode.datectRange;


            // 读取探测location
            domparser.readXml(domparser.findSubNode(deviceNodeList.at(i), "location" ,ok), deviceNode.location );
            if( !ok )
            {
                if(IF_DEBUG)
                {
                    qDebug() << "UltrasonicSensor::initialSensorByConfigFile ==== 读取设备探测location失败:" \
                             << deviceNode.location;
                }
                return -14;
            }
            qDebug() << "UltrasonicSensor::initialSensorByConfigFile ====detectRange:" << deviceNode.location;

            mutex.lock();
            deviceNodeMap.insert(deviceNode.deviceId, deviceNode);
            deviceRequestCount.insert(deviceNode.deviceId, 0);
            mutex.unlock();
            qDebug() << "initialSensorByConfigFile ==== deviceAddress:" \
                     << deviceNodeMap.value(deviceNode.deviceId).deviceId;
        }

    domparser.closeXml();
    isInitialOk = true;

    return 1;

}

/**
 * @brief UltrasonicSensor::setSensor485Address     修改设备的RS485串口地址
 * @param deviceId       设备原地址，默认0xe8
 * @param addressIn      设备目标地址
 * @return             成功返回１，失败返回－１
 */
int UltrasonicSensor::setSensor485Address(int origineAddressIn, int addressIn)
{
    if( serialPort_device->isOpen() )
    {
//            if( (validAddressList.contains(addressIn)) && (validAddressList.contains(origineAddressIn)) )
//            {
                QByteArray sendFirstTiming;
                sendFirstTiming.resize(12);
                qDebug() << "deviceNodeMap.value(deviceId).deviceAddress:" \
                         << origineAddressIn;

                sendFirstTiming[0] = origineAddressIn;
                sendFirstTiming[1] = 0x02;
                sendFirstTiming[2] = 0x9a;

                // 写地址
                serialPort_device->write(sendFirstTiming);
                usleep(1000);
                sendFirstTiming[2] = 0x92;
                serialPort_device->write(sendFirstTiming);
                usleep(1000);
                sendFirstTiming[2] = 0x9e;
                serialPort_device->write(sendFirstTiming);
                usleep(1000);
                sendFirstTiming[2] = addressIn;
                serialPort_device->write(sendFirstTiming);
                usleep(100000);

                UltrasonicDeviceNode newAddress;
                mutex.lock();
                newAddress.datectRange = deviceNodeMap.value(origineAddressIn).datectRange;
                newAddress.powerNoiseClass = deviceNodeMap.value(origineAddressIn).powerNoiseClass;
                newAddress.deviceId = addressIn;

                deviceNodeMap.remove(origineAddressIn);

                deviceNodeMap.insert(addressIn, newAddress);

                mutex.unlock();

                qDebug() << "The origineAddress :" << origineAddressIn << "====The newAddress:" << addressIn;
                qDebug() << "UltrasonicSensor::setSensor485Address ==== Rs485地址写入成功!";
                return 1;
//            }
//            else
//            {
//                qDebug() << "UltrasonicSensor::setSensor485Address ==== 地址输入有误！";
//                return -1;
//            }
    }
    else
    {
         qDebug() << "UltrasonicSensor::setSensor485Address ==== 串口未打开!" << serialConfig->serialDeviceName;

         return -1;
    }

    return 1;
}

/**
 * @brief UltrasonicSensor::setPowerSupplyNoiseClass    设置设备电源降噪级别
 * @param deviceId      设备编号
 * @param classIn       电源降噪级别(1-7)
 * @return      成功返回１，失败返回－１
 */
int UltrasonicSensor::setPowerSupplyNoiseClass(int devicdId, EM_POWER_NOISE_CLASS classIn)
{
    // 检测降噪电源类别
    if( validAddressList.contains(devicdId))
    {
        unsigned int command;
        if( classIn  == EM_POWER_NOISE_CLASS_1)
        {
            command = 0x70;
        }
        else if( classIn== EM_POWER_NOISE_CLASS_2 )
        {
            command = 0x71;
        }
        else if( classIn == EM_POWER_NOISE_CLASS_3 )
        {
            command = 0x72;
        }
        else if( classIn == EM_POWER_NOISE_CLASS_4 )
        {
            command = 0x73;
        }
        else if( classIn == EM_POWER_NOISE_CLASS_5 )
        {
            command = 0x74;
        }
        else if( classIn == EM_POWER_NOISE_CLASS_6 )
        {
            command = 0x75;
        }

        else
        {
            qDebug() << "UltrasonicSensor::setPowerSupplyNoiseClass === 降噪电源参数有误:" << classIn;
            return -1;
        }

        QByteArray sendData;
        sendData.resize(3);
        mutex.lock();
        sendData[0] = deviceNodeMap.value(devicdId).deviceId;
        mutex.unlock();
        sendData[1] = 0x02;
        sendData[2] = command;

        serialPort_device->write(sendData);

        // 延时2100ｍｓ
        usleep(2100000);

        //why cause iterator ++ coredump
//        UltrasonicDeviceNode newDevice;
//        mutex.lock();
//        newDevice.datectRange = deviceNodeMap.value(devicdId).datectRange;
//        newDevice.deviceId = deviceNodeMap.value(devicdId).deviceId;
//        newDevice.powerNoiseClass = classIn;
//        deviceNodeMap[devicdId] = newDevice;
//        mutex.unlock();

        qDebug() << "UltrasonicSensor::setPowerSupplyNoiseClass === 电源降噪设置成功:" << command;
    }
    else
    {

            qDebug() << "UltrasonicSensor::setPowerSupplyNoiseClass ==== 地址参数有误！" << devicdId;
            return -1;
    }

    return 1;
}

int UltrasonicSensor::setUltrasonicPowerSupplyNoiseClass(EM_POWER_NOISE_CLASS classIn)
{
    mutex.lock();
    QMapIterator<int, UltrasonicDeviceNode> i(deviceNodeMap);
    mutex.unlock();
    while (i.hasNext())
    {
        i.next();
        setPowerSupplyNoiseClass(i.key(),classIn);
    }

    return 1;
}


/**
 * @brief UltrasonicSensor::setDetectDistanceRange  设置模块探测范围
 * @param deviceId          设备编号
 * @param detectRangeIn     探测范围（单位：ｍｍ）
 * @return      成功返回１，失败返回－１
 */
int UltrasonicSensor::setDetectDistanceRange(int devicedId, double detectRangeIn)
{
    if( validAddressList.contains(devicedId) )
    {
        if( (detectRangeIn >= 0) && (detectRangeIn <= 11000))
        {
            UltrasonicDeviceNode newDevice;

            mutex.lock();
            newDevice.datectRange = detectRangeIn;
            newDevice.deviceId = deviceNodeMap.value(devicedId).deviceId;
            newDevice.powerNoiseClass = deviceNodeMap.value(devicedId).powerNoiseClass;
            deviceNodeMap[devicedId] = newDevice;
            mutex.unlock();

            qDebug() << "探测范围设置成功!";
        }
        else
        {
            qDebug() << "探测范围设置失败！";
            return -1;
        }
    }
    else
    {
        qDebug() << "UltrasonicSensor::setDetectDistanceRange ==== 地址参数有误:" << devicedId;
        return -1;
    }
    return 1;
}

// 获取探测数据
int UltrasonicSensor::getDetectDistanceFromBuffer(int devicdId, double &distanceResultOut)
{

    mutex.lock();
    if( false == sensorDistanceValue.contains(devicdId) )
    {
        mutex.unlock();
        return -1;
    }
    distanceResultOut = sensorDistanceValue.value(devicdId);
    mutex.unlock();
    if(1!=isOneDeviceRequestResponseOk(devicdId))
    {
        return -2;
    }
    return 1;
}

int UltrasonicSensor::getDetectDistanceFromBuffer(QVector<double> &distanceResultOut)
{
    distanceResultOut.clear();
    mutex.lock();
    QMapIterator<int, double> i(sensorDistanceValue);
    while (i.hasNext())
    {
        i.next();
        distanceResultOut.append(sensorDistanceValue[i.key()]);

    }

    mutex.unlock();
}

int UltrasonicSensor::getDetectDistanceFromBuffer(int locationIn, QVector<double> &distanceResultOut)
{
    distanceResultOut.clear();
    mutex.lock();
    QMapIterator<int, double> i(sensorDistanceValue);
    while (i.hasNext())
    {
        i.next();
        if(locationIn==deviceNodeMap.value(i.key()).location)
        {
            distanceResultOut.append(sensorDistanceValue[i.key()]);
        }
    }

    mutex.unlock();
    return 1;
}

int UltrasonicSensor::getSensorName(std::vector<string> &sensorNames)
{
    mutex.lock();
    sensorNames.clear();
    QMapIterator<int, double> i(sensorDistanceValue);
    string tmpName;
    while (i.hasNext())
    {
        i.next();
//        tmpName=std::to_string(deviceNodeMap.value(i.key()).location);
        switch(deviceNodeMap.value(i.key()).location)
        {
        case 1:
        {
           tmpName="前";
           break;
        }
        case 2:
        {
           tmpName="后";
           break;
        }
        case 3:
        {
           tmpName="左";
           break;
        }
        case 4:
        {
           tmpName="右";
           break;
        }
        }
        sensorNames.push_back(tmpName);
    }
    mutex.unlock();
    return 1;
}

int UltrasonicSensor::getSensorStatus(QVector<int> &deviceRequestCountOut)
{
    deviceRequestCountOut.clear();
    mutex.lock();
    QMapIterator<int, int> i(deviceRequestCount);
    while (i.hasNext())
    {
        i.next();
        deviceRequestCountOut.append(deviceRequestCount[i.key()]);
    }
    mutex.unlock();
    return 1;
}



// 定时器更新槽函数
void UltrasonicSensor::timeOut_slot()
{
    if(1==debugFlag)
     {
         qDebug()<<"UltrasonicSensor::timeOut_slot() In";
     }
     updateAllSensorDistanceValue();
     if(1==debugFlag)
     {
         qDebug()<<"UltrasonicSensor::timeOut_slot() Out";
     }

}

int UltrasonicSensor::getLocationSensorSafeRatioOutput(int locationIn, double &safeSpeedRatioOut)
{
    QVector<double> tmpDistance;
    getDetectDistanceFromBuffer(locationIn,tmpDistance);
    for(int i=0;i<tmpDistance.size();i++)
    {
       if(tmpDistance[i]<barrierStopDistance)
       {
           safeSpeedRatioOut=0.0;
           sensorSafeStatus[locationIn-1]=safeSpeedRatioOut;
           return 1;
       }
       else if(tmpDistance[i]<barrierSlowDownDistance)
       {
           safeSpeedRatioOut=barrierSlowDownRatio;
           sensorSafeStatus[locationIn-1]=safeSpeedRatioOut;
           return 1;
       }
    }
    safeSpeedRatioOut=1.0;
    sensorSafeStatus[locationIn-1]=safeSpeedRatioOut;
    return 1;
}


// 探测数据更新
int UltrasonicSensor::updateAllSensorDistanceValue()
{
//    qDebug() << "UltrasonicSensor::updateAllSensorDistanceValue ===== it.key():" << it.key();

//    mutex.lock();
//    int tryCount=0;
//    int tmpReturn=1;
//    while(false==sensorEnable[iterator_currentNode.key()])//???
//    {

//        if( iterator_currentNode.key() == deviceNodeMap.lastKey() )
//        {
//            iterator_currentNode = deviceNodeMap.constBegin();
//        }
//        else
//        {
//            ++iterator_currentNode;
//        }

//        tryCount++;
//        if(tryCount>deviceNodeMap.size())
//        {
//            tmpReturn= -1;
//        }
//    }

//    mutex.unlock();
    //4次进入都没有收到才再次发送，避免返回信号干扰到相邻的传感器（错误近距离）
    mutex.lock();
    if(0==deviceRequestCount[iterator_currentNode.key()%5])
    {
        sendGetValueCommand( iterator_currentNode.key() );
        deviceRequestCount[iterator_currentNode.key()]++;
    }
    else
    {
        deviceRequestCount[iterator_currentNode.key()]++;
    }
    mutex.unlock();

    return 1;
}


// 发送探测指令函数
int UltrasonicSensor::sendGetValueCommand(int deviceId)
{
//    mutex.lock();
//    qDebug() << "============================= 开始发送探测指令 ===========================";
    QByteArray sendCommand;
    sendCommand.resize(3);

    sendCommand[0] = deviceId;
//    mutex.unlock();

    sendCommand[1] = 0x02;
    sendCommand[2] = 0;

    // 发送探测指令
//    qDebug() << "UltrasonicSensor::sendGetValueCommand ====currentDeviceId:" << deviceId;
//    qDebug() << "UltrasonicSensor::sendGetValueCommand ==== new_serialAddress:" << deviceNodeMap.value(deviceId).deviceId;

    if( ( deviceNodeMap.value(deviceId).datectRange >= 0) && ( deviceNodeMap.value(deviceId).datectRange <= 500 ))
    {
        // 0-0.5m 探测指令,，返回ｕｓ时间
        sendCommand[2] = 0x05;

    }
    else if( ( deviceNodeMap.value(deviceId).datectRange >= 0) && ( deviceNodeMap.value(deviceId).datectRange <= 1000 ))
    {
        // 0-1m 探测指令
        sendCommand[2] = 0x0a;

    }
    else if( ( deviceNodeMap.value(deviceId).datectRange >= 0) && ( deviceNodeMap.value(deviceId).datectRange <= 2000 ))
    {
        // 0-2m 探测指令
        sendCommand[2] = 0x14;
    }
    else if(( deviceNodeMap.value(deviceId).datectRange >0) && (deviceNodeMap.value(deviceId).datectRange <= 3000 ))
    {
        // 0-3m 探测指令
        sendCommand[2] = 0x1e;
    }
    else
    {
        // 0-4m 探测指令
        sendCommand[2] = 0x28;
    }

     serialPort_device->write(sendCommand);
     serialPort_device->flush();


    // 延时10ms
//    usleep(10000);

//    qDebug() << "----------------------------- 探测指令完成 -----------------------------";
//        mutex.unlock();
    return 1;
}

int UltrasonicSensor::isAllSensorRequestResponseOk(int &deviceIdOut)
{
    mutex.lock();
    QMapIterator<int, int> i(deviceRequestCount);
    while (i.hasNext())
    {
        if(i.next().value() >8)
        {
            deviceIdOut=i.key();
            mutex.unlock();
            return -1;
        }
    }

    mutex.unlock();

    return 1;

}

int UltrasonicSensor::isOneDeviceRequestResponseOk(int deviceIdIn)
{
    mutex.lock();

    if(deviceRequestCount.value(deviceIdIn)>3)
    {
        qDebug()<<"response error,deviceIdIn="<<deviceIdIn<<",count="
               <<deviceRequestCount.value(deviceIdIn);
        mutex.unlock();
        return -1;
    }
    else
    {
        mutex.unlock();
        return 1;
    }

    return 1;

}

int UltrasonicSensor::ultrasonicSensorProtectLoop(E_BARRIER_TETECT_MODE validModeIn, double &safeSpeedRatioOut)
{
    if(false==isDetectEnable)
    {
        safeSpeedRatioOut=1.0;
        sensorSafeStatus[0]=1.0;
        sensorSafeStatus[1]=1.0;
        sensorSafeStatus[2]=1.0;
        sensorSafeStatus[3]=1.0;
        return 1;
    }

    int sensorLocation;
    switch(validModeIn)
    {
    case E_BARRIER_TETECT_MODE_NONE:
    {
        safeSpeedRatioOut=1.0;
        sensorSafeStatus[0]=1.0;
        sensorSafeStatus[1]=1.0;
        sensorSafeStatus[2]=1.0;
        sensorSafeStatus[3]=1.0;
        return 1;
        break;
    }
    case E_BARRIER_TETECT_MODE_FRONT:
    {
        sensorSafeStatus[1]=1.0;
        sensorSafeStatus[2]=1.0;
        sensorSafeStatus[3]=1.0;
        sensorLocation=1;
        return getLocationSensorSafeRatioOutput(sensorLocation,safeSpeedRatioOut);
        break;
    }
    case E_BARRIER_TETECT_MODE_BACK:
    {
        sensorSafeStatus[0]=1.0;
        sensorSafeStatus[2]=1.0;
        sensorSafeStatus[3]=1.0;
        sensorLocation=2;
        return getLocationSensorSafeRatioOutput(sensorLocation,safeSpeedRatioOut);
        break;
    }
    case E_BARRIER_TETECT_MODE_LEFT:
    {
        sensorSafeStatus[0]=1.0;
        sensorSafeStatus[1]=1.0;
        sensorSafeStatus[3]=1.0;
        sensorLocation=3;
        return getLocationSensorSafeRatioOutput(sensorLocation,safeSpeedRatioOut);
        break;
    }
    case E_BARRIER_TETECT_MODE_RIGHT:
    {
        sensorSafeStatus[0]=1.0;
        sensorSafeStatus[1]=1.0;
        sensorSafeStatus[2]=1.0;
        sensorLocation=4;
        return getLocationSensorSafeRatioOutput(sensorLocation,safeSpeedRatioOut);
        break;
    }
    case E_BARRIER_TETECT_MODE_ROTATION:
    {
        QVector<double> tmpSafeRatio;
        tmpSafeRatio.resize(4);
        getLocationSensorSafeRatioOutput(1,tmpSafeRatio[0]);
        getLocationSensorSafeRatioOutput(2,tmpSafeRatio[1]);
        getLocationSensorSafeRatioOutput(3,tmpSafeRatio[2]);
        getLocationSensorSafeRatioOutput(4,tmpSafeRatio[3]);
        safeSpeedRatioOut=1;
        for(int i=0;i<tmpSafeRatio.size();i++)
        {
            if(tmpSafeRatio[i]<safeSpeedRatioOut)
            {
                safeSpeedRatioOut=tmpSafeRatio[i];
            }
        }
        return 1;
        break;
    }
    case E_BARRIER_TETECT_MODE_ALL:
    {
        QVector<double> tmpSafeRatio;
        tmpSafeRatio.resize(4);
        getLocationSensorSafeRatioOutput(1,tmpSafeRatio[0]);
        getLocationSensorSafeRatioOutput(2,tmpSafeRatio[1]);
        getLocationSensorSafeRatioOutput(3,tmpSafeRatio[2]);
        getLocationSensorSafeRatioOutput(4,tmpSafeRatio[3]);
        safeSpeedRatioOut=1;
        for(int i=0;i<tmpSafeRatio.size();i++)
        {
            if(tmpSafeRatio[i]<safeSpeedRatioOut)
            {
                safeSpeedRatioOut=tmpSafeRatio[i];
            }
        }
        return 1;
        break;
    }
    case E_BARRIER_TETECT_MODE_FRONT_LEFT:
    {
        sensorSafeStatus[1]=1.0;
        sensorSafeStatus[3]=1.0;
        double tmpRatio1,tmpRatio2;
        getLocationSensorSafeRatioOutput(1,tmpRatio1);
        getLocationSensorSafeRatioOutput(3,tmpRatio2);
        if(tmpRatio1<=tmpRatio2)
        {
            safeSpeedRatioOut=tmpRatio1;
        }
        else
        {
           safeSpeedRatioOut=tmpRatio2;
        }
        break;
    }
    case E_BARRIER_TETECT_MODE_FRONT_RIGHT:
    {
        sensorSafeStatus[1]=1.0;
        sensorSafeStatus[2]=1.0;
        double tmpRatio1,tmpRatio2;
        getLocationSensorSafeRatioOutput(1,tmpRatio1);
        getLocationSensorSafeRatioOutput(4,tmpRatio2);
        if(tmpRatio1<=tmpRatio2)
        {
            safeSpeedRatioOut=tmpRatio1;
        }
        else
        {
           safeSpeedRatioOut=tmpRatio2;
        }
        break;
    }
    case E_BARRIER_TETECT_MODE_BACK_LEFT:
    {
        sensorSafeStatus[0]=1.0;
        sensorSafeStatus[3]=1.0;
        double tmpRatio1,tmpRatio2;
        getLocationSensorSafeRatioOutput(2,tmpRatio1);
        getLocationSensorSafeRatioOutput(3,tmpRatio2);
        if(tmpRatio1<=tmpRatio2)
        {
            safeSpeedRatioOut=tmpRatio1;
        }
        else
        {
           safeSpeedRatioOut=tmpRatio2;
        }
        break;
    }
    case E_BARRIER_TETECT_MODE_BACK_RIGHT:
    {
        sensorSafeStatus[0]=1.0;
        sensorSafeStatus[2]=1.0;
        double tmpRatio1,tmpRatio2;
        getLocationSensorSafeRatioOutput(2,tmpRatio1);
        getLocationSensorSafeRatioOutput(4,tmpRatio2);
        if(tmpRatio1<=tmpRatio2)
        {
            safeSpeedRatioOut=tmpRatio1;
        }
        else
        {
           safeSpeedRatioOut=tmpRatio2;
        }
        break;
    }
    }

    return 1;

}


// 接收探测数据函数
int UltrasonicSensor::receiveDetectData()
{
     mutex.lock();
//    qDebug() << "----------------------------- 开始接收数据 -----------------------------";
    QByteArray receiveByteArrayData;
    receiveByteArrayData.resize(2);

    unsigned char highByte = 0x00;
    unsigned char lowByte = 0x00;

    unsigned short receiveData = 0;

    if( serialPort_device->bytesAvailable() >= 2 )
    {
        receiveByteArrayData  = serialPort_device->readAll();

//        qDebug() << "recereceiveByteArrayData.toHex ====" << receiveByteArrayData.toHex();
//        qDebug() << "recereceiveByteArrayData[0] ====" << receiveByteArrayData[0];
//        qDebug() << "recereceiveByteArrayData[1] ====" << receiveByteArrayData[1];

        highByte = receiveByteArrayData.at(0);
        lowByte = receiveByteArrayData.at(1);

        receiveData |= highByte;
//        qDebug() << "receiveData ==== " << receiveData;
        receiveData = ( receiveData << 8 ) | lowByte;

        if( receiveByteArrayData != NULL )
        {

//            mutex.lock();
            //
            double  tmpSoundSpeed=346;//m/s
            sensorDistanceValue[ iterator_currentNode.key() ] = tmpSoundSpeed/1000.0*((double)receiveData)/2.0;
//            sensorDistanceValue[ it.key() ] = receiveData;
            deviceRequestCount[iterator_currentNode.key()]=0;
//            mutex.unlock();
//            qDebug() << "UltrasonicSensor::receiveDetectData ==== 探测地址：" \
//                     << it.key() << "-------探测数据："<< receiveData ;
//            qDebug() << "  -----设备地址：" << deviceNodeMap.value( it.key() ).deviceId;

            receiveData = 0;
            receiveByteArrayData.fill(0);
            serialPort_device->flush();

//            qDebug() << "============================= 接收完成 =============================" ;

//            mutex.lock();
            if( iterator_currentNode.key() == deviceNodeMap.lastKey() )
            {
                iterator_currentNode = deviceNodeMap.constBegin();
            }
            else
            {

                ++iterator_currentNode;

                mutex.unlock();
                return 1;
            }
//            qDebug() << "UltrasonicSensor::receiveDetectData ==== it.key():" << it.key();

        }
        else
        {
            receiveByteArrayData.clear();
        }

    }
    mutex.unlock();
    return 1;
}

// 串口配置及初始化
 int UltrasonicSensor::initalSerial()
 {
     //serialPort->close();
     bool isFindCom=false;
     /**
      * @brief serialPortInfo        串口信息
      */
     QSerialPortInfo serialPortInfo;


         foreach ( serialPortInfo, serialPortInfo.availablePorts() )
         {
             qDebug() << "portName : " << serialPortInfo.portName();
             qDebug() << "Description : " << serialPortInfo.description();
             qDebug() << "serialNumber: " << serialPortInfo.serialNumber();
             qDebug() << "productIdentifier: " << serialPortInfo.productIdentifier();
             if(1==serialConfig->isUseUsbDeviceSerialCode)
             {
                 if( serialPortInfo.serialNumber() == serialConfig->serialDeviceName)
                 {
                     // 设置端口号
                     serialPort_device->setPortName(serialPortInfo.portName());
                     qDebug()<<"find COM"<<serialPortInfo.portName();
                     isFindCom=true;
                     break;
                 }
             }
             else
             {
                 if( serialPortInfo.portName() == serialConfig->serialDeviceName )
                 {
                     // 设置端口号
                     serialPort_device->setPortName(serialPortInfo.portName());
                     qDebug()<<"find COM"<<serialPortInfo.portName();
                     isFindCom=true;
                     break;
                 }
             }

     }



     if(false==isFindCom)
     {
         qDebug() << "没有找到指定串口:" << serialConfig->serialDeviceName;
          return -1;
     }

     qDebug() << "serilaPortName:" << serialPort_device->portName();

     if( serialPort_device->open(QIODevice::ReadWrite) )
     {
        // 配置串口参数
        serialPort_device->setBaudRate(serialConfig->baudRate);         //　波特率
        serialPort_device->setDataBits(serialConfig->dataBits);         //  数据位
        serialPort_device->setParity(serialConfig->parity);             //  校验位
        serialPort_device->setStopBits(serialConfig->stopBits);         //  停止位
        serialPort_device->setFlowControl(QSerialPort::NoFlowControl);  //  流控制
        qDebug() << "串口打开成功:" << serialPort_device->portName();
    }
    else
     {
        qDebug() << "串口打开失败:" << serialPort_device->portName();
         return -1;
     }

     return 1;
 }


 void UltrasonicSensor::addMsg(int messageLevel, string componentName, string messageType,
                            int messageCode, int robotId,
                            int parameter1, int parameter2, int parameter3, int parameter4)
 {
     Message tmpMsg;
     tmpMsg.MessageLevel = messageLevel;
     tmpMsg.componentClassName = componentName;
     tmpMsg.messageType = messageType;
     tmpMsg.robotId = robotId;
     tmpMsg.messageCode = messageCode;



     QString infomationStr;

     switch(messageCode)
     {

     case 3851:
     {
         infomationStr =  QObject::tr("超声波设备的串口打开失败！");
         break;
     }
     case 3850:
     {
         infomationStr =  QObject::tr("超声波设备配置文件缺少参数！");
         break;
     }


         break;
     }


     tmpMsg.MessageInformation = infomationStr.toStdString();

     MessageLog::getInstance()->addMessage(tmpMsg);
 }
